"""

Copyright MUAMP 2021 MUAMP.COM David French

This Circuit Python code to control the Robot Quadruped "PICO Python Dog" can be freely used and modified, providing:

This code must NOT be used for commercial use or for profit.

This comment must remain in the code at the top of the .py file and this comment must NOT be modified or removed.

"""

import time

import board

import busio

import pwmio

from adafruit_motor import servo

import adafruit_icm20x

from math import asin, acos, atan, degrees, sqrt

import pulseio

import adafruit_irremote

# IR sensor

IR_PIN = board.GP9 # Pin GP9 connected to IR receiver

# IR decoder

pulsein = pulseio.PulseIn(IR_PIN, maxlen=100, idle_state=True)

decoder = adafruit_irremote.GenericDecode()

pulsein.clear()

pulsein.resume()

# Acc Gyro Mag test setup on i2c (1)

i2c = busio.I2C(board.GP11, board.GP10)

icm = adafruit_icm20x.ICM20948(i2c)

# AGM functions

def Acc(): # Acc +/- 10 for 90 degrees of x and y axis (wobble board +...)

Ax = round(icm.acceleration[0]) # Gyro detects sudden movement in real time (compliance +...)

Ay = round(icm.acceleration[1]) # Mag (compass) for up side down detection

return Ax, Ay

# Servo setup (Raspberry Pi PICO micro controller (Circuit Python) with Tower Pro MG90S servos

FL_knee = servo.Servo(pwmio.PWMOut(board.GP0, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FL_hip = servo.Servo(pwmio.PWMOut(board.GP1, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FR_knee = servo.Servo(pwmio.PWMOut(board.GP27, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

FR_hip = servo.Servo(pwmio.PWMOut(board.GP26, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BL_knee = servo.Servo(pwmio.PWMOut(board.GP14, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BL_hip = servo.Servo(pwmio.PWMOut(board.GP15, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BR_knee = servo.Servo(pwmio.PWMOut(board.GP21, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

BR_hip = servo.Servo(pwmio.PWMOut(board.GP20, duty_cycle=0, frequency=50), min_pulse=500, max_pulse=2400)

# servo function - converts Cartesian inputs to degrees of angles of servos

def leg_control(FLx,FLy,FRx,FRy,BLx,BLy,BRx,BRy):

FRcal = -5 # calibration factor

FLhip, FLknee = rec2pol(FLx, FLy) # This IK code is interpolated by default!

FRhip, FRknee = rec2pol(FRx, FRy)

BLhip, BLknee = rec2pol(BLx, BLy)

BRhip, BRknee = rec2pol(BRx, BRy)

# degrees do NOT have to be integers

FL_knee.angle = 180 - FLknee

FL_hip.angle = 180 - FLhip

FR_knee.angle = FRknee + FRcal

FR_hip.angle = FRhip

BL_knee.angle = 180 - BLknee

BL_hip.angle = 180 - BLhip

BR_knee.angle = BRknee

BR_hip.angle = BRhip

def rec2pol(x,y): # upper and lower leg are both 50mm

hypotenuse = sqrt(y**2+x**2)

hip = 90+(degrees((atan(x/y))-acos(((hypotenuse**2)/(100*hypotenuse)))))

knee = degrees(acos(1-((hypotenuse**2)/5000)))

return hip, knee

def numbers(start, finish): # list of numbers from 'start' to 'finish' for smooth linear leg movement

numbers = []

if start < finish:

for new_number in range(start, finish, 1):

for fraction in range(0, 10, 2):

value = new_number + fraction/10

numbers.append(value)

else:

for new_number in range(start-1, finish-1, -1):

for fraction in range(10, 0, -2):

value = new_number + fraction/10

numbers.append(value)

numbers.append(finish+0.0)

return numbers

""" MAIN CODE """

time.sleep(3)

# CRAWL

# leg_control(FLx, FLy, FRx, FRy, BLx, BLy, BRx, BRy)

delay = 0.002

while True:

leg_control(-40, 60, 40, 60, 40, 60, -40, 60)

time.sleep(delay)

leg_control(-39, 59.4, 39, 60, 39, 60, -39, 59.4)

time.sleep(delay)

leg_control(-38, 58.6, 38, 60, 38, 60, -38, 58.6)

time.sleep(delay)

leg_control(-37, 57.8, 37, 60, 37, 60, -37, 57.8)

time.sleep(delay)

leg_control(-36, 57, 36, 60, 36, 60, -36, 57)

time.sleep(delay)

leg_control(-35, 56.3, 35, 60, 35, 60, -35, 56.3)

time.sleep(delay)

leg_control(-34, 55.6, 34, 60, 34, 60, -34, 55.6)

time.sleep(delay)

leg_control(-33, 54.9, 33, 60, 33, 60, -33, 54.9)

time.sleep(delay)

leg_control(-32, 54.2, 32, 60, 32, 60, -32, 54.2)

time.sleep(delay)

leg_control(-31, 53.6, 31, 60, 31, 60, -31, 53.6)

time.sleep(delay)

leg_control(-30, 53, 30, 60, 30, 60, -30, 53)

time.sleep(delay)

leg_control(-29, 52.5, 29, 60, 29, 60, -29, 52.5)

time.sleep(delay)

leg_control(-28, 51.9, 28, 60, 28, 60, -28, 51.9)

time.sleep(delay)

leg_control(-27, 51.4, 27, 60, 27, 60, -27, 51.4)

time.sleep(delay)

leg_control(-26, 50.9, 26, 60, 26, 60, -26, 50.9)

time.sleep(delay)

leg_control(-25, 50.5, 25, 60, 25, 60, -25, 50.5)

time.sleep(delay)

leg_control(-24, 50, 24, 60, 24, 60, -24, 50)

time.sleep(delay)

leg_control(-23, 49.6, 23, 60, 23, 60, -23, 49.6)

time.sleep(delay)

leg_control(-22, 49.2, 22, 60, 22, 60, -22, 49.2)

time.sleep(delay)

leg_control(-21, 48.8, 21, 60, 21, 60, -21, 48.8)

time.sleep(delay)

leg_control(-20, 48.4, 20, 60, 20, 60, -20, 48.4)

time.sleep(delay)

leg_control(-19, 48.1, 19, 60, 19, 60, -19, 48.1)

time.sleep(delay)

leg_control(-18, 47.8, 18, 60, 18, 60, -18, 47.8)

time.sleep(delay)

leg_control(-17, 47.5, 17, 60, 17, 60, -17, 47.5)

time.sleep(delay)

leg_control(-16, 47.2, 16, 60, 16, 60, -16, 47.2)

time.sleep(delay)

leg_control(-15, 46.9, 15, 60, 15, 60, -15, 46.9)

time.sleep(delay)

leg_control(-14, 46.7, 14, 60, 14, 60, -14, 46.7)

time.sleep(delay)

leg_control(-13, 46.4, 13, 60, 13, 60, -13, 46.4)

time.sleep(delay)

leg_control(-12, 46.2, 12, 60, 12, 60, -12, 46.2)

time.sleep(delay)

leg_control(-11, 46, 11, 60, 11, 60, -11, 46)

time.sleep(delay)

leg_control(-10, 45.8, 10, 60, 10, 60, -10, 45.8)

time.sleep(delay)

leg_control(-9, 45.7, 9, 60, 9, 60, -9, 45.7)

time.sleep(delay)

leg_control(-8, 45.5, 8, 60, 8, 60, -8, 45.5)

time.sleep(delay)

leg_control(-7, 45.4, 7, 60, 7, 60, -7, 45.4)

time.sleep(delay)

leg_control(-6, 45.3, 6, 60, 6, 60, -6, 45.3)

time.sleep(delay)

leg_control(-5, 45.2, 5, 60, 5, 60, -5, 45.2)

time.sleep(delay)

leg_control(-4, 45.1, 4, 60, 4, 60, -4, 45.1)

time.sleep(delay)

leg_control(-3, 45.1, 3, 60, 3, 60, -3, 45.1)

time.sleep(delay)

leg_control(-2, 45, 2, 60, 2, 60, -2, 45)

time.sleep(delay)

leg_control(-1, 45, 1, 60, 1, 60, -1, 45)

time.sleep(delay)

leg_control(0, 45, 0, 60, 0, 60, 0, 45)

time.sleep(delay)

leg_control(1, 45, -1, 60, -1, 60, 1, 45)

time.sleep(delay)

leg_control(2, 45, -2, 60, -2, 60, 2, 45)

time.sleep(delay)

leg_control(3, 45.1, -3, 60, -3, 60, 3, 45.1)

time.sleep(delay)

leg_control(4, 45.1, -4, 60, -4, 60, 4, 45.1)

time.sleep(delay)

leg_control(5, 45.2, -5, 60, -5, 60, 5, 45.2)

time.sleep(delay)

leg_control(6, 45.3, -6, 60, -6, 60, 6, 45.3)

time.sleep(delay)

leg_control(7, 45.4, -7, 60, -7, 60, 7, 45.4)

time.sleep(delay)

leg_control(8, 45.5, -8, 60, -8, 60, 8, 45.5)

time.sleep(delay)

leg_control(9, 45.7, -9, 60, -9, 60, 9, 45.7)

time.sleep(delay)

leg_control(10, 45.8, -10, 60, -10, 60, 10, 45.8)

time.sleep(delay)

leg_control(11, 46, -11, 60, -11, 60, 11, 46)

time.sleep(delay)

leg_control(12, 46.2, -12, 60, -12, 60, 12, 46.2)

time.sleep(delay)

leg_control(13, 46.4, -13, 60, -13, 60, 13, 46.4)

time.sleep(delay)

leg_control(14, 46.7, -14, 60, -14, 60, 14, 46.7)

time.sleep(delay)

leg_control(15, 46.9, -15, 60, -15, 60, 15, 46.9)

time.sleep(delay)

leg_control(16, 47.2, -16, 60, -16, 60, 16, 47.2)

time.sleep(delay)

leg_control(17, 47.5, -17, 60, -17, 60, 17, 47.5)

time.sleep(delay)

leg_control(18, 47.8, -18, 60, -18, 60, 18, 47.8)

time.sleep(delay)

leg_control(19, 48.1, -19, 60, -19, 60, 19, 48.1)

time.sleep(delay)

leg_control(20, 48.4, -20, 60, -20, 60, 20, 48.4)

time.sleep(delay)

leg_control(21, 48.8, -21, 60, -21, 60, 21, 48.8)

time.sleep(delay)

leg_control(22, 49.2, -22, 60, -22, 60, 22, 49.2)

time.sleep(delay)

leg_control(23, 49.6, -23, 60, -23, 60, 23, 49.6)

time.sleep(delay)

leg_control(24, 50, -24, 60, -24, 60, 24, 50)

time.sleep(delay)

leg_control(25, 50.5, -25, 60, -25, 60, 25, 50.5)

time.sleep(delay)

leg_control(26, 50.9, -26, 60, -26, 60, 26, 50.9)

time.sleep(delay)

leg_control(27, 51.4, -27, 60, -27, 60, 27, 51.4)

time.sleep(delay)

leg_control(28, 51.9, -28, 60, -28, 60, 28, 51.9)

time.sleep(delay)

leg_control(29, 52.5, -29, 60, -29, 60, 29, 52.5)

time.sleep(delay)

leg_control(30, 53, -30, 60, -30, 60, 30, 53)

time.sleep(delay)

leg_control(31, 53.6, -31, 60, -31, 60, 31, 53.6)

time.sleep(delay)

leg_control(32, 54.2, -32, 60, -32, 60, 32, 54.2)

time.sleep(delay)

leg_control(33, 54.9, -33, 60, -33, 60, 33, 54.9)

time.sleep(delay)

leg_control(34, 55.6, -34, 60, -34, 60, 34, 55.6)

time.sleep(delay)

leg_control(35, 56.3, -35, 60, -35, 60, 35, 56.3)

time.sleep(delay)

leg_control(36, 57, -36, 60, -36, 60, 36, 57)

time.sleep(delay)

leg_control(37, 57.8, -37, 60, -37, 60, 37, 57.8)

time.sleep(delay)

leg_control(38, 58.6, -38, 60, -38, 60, 38, 58.6)

time.sleep(delay)

leg_control(39, 59.4, -39, 60, -39, 60, 39, 59.4)

time.sleep(delay)

leg_control(40, 60, -40, 60, -40, 60, 40, 60)

time.sleep(delay)

# half way point

leg_control(40, 60, -40, 60, -40, 60, 40, 60)

time.sleep(delay)

leg_control(39, 60, -39, 59.4, -39, 59.4, 39, 60)

time.sleep(delay)

leg_control(38, 60, -38, 58.6, -38, 58.6, 38, 60)

time.sleep(delay)

leg_control(37, 60, -37, 57.8, -37, 57.8, 37, 60)

time.sleep(delay)

leg_control(36, 60, -36, 57, -36, 57, 36, 60)

time.sleep(delay)

leg_control(35, 60, -35, 56.3, -35, 56.3, 35, 60)

time.sleep(delay)

leg_control(34, 60, -34, 55.6, -34, 55.6, 34, 60)

time.sleep(delay)

leg_control(33, 60, -33, 54.9, -33, 54.9, 33, 60)

time.sleep(delay)

leg_control(32, 60, -32, 54.2, -32, 54.2, 32, 60)

time.sleep(delay)

leg_control(31, 60, -31, 53.6, -31, 53.6, 31, 60)

time.sleep(delay)

leg_control(30, 60, -30, 53, -30, 53, 30, 60)

time.sleep(delay)

leg_control(29, 60, -29, 52.5, -29, 52.5, 29, 60)

time.sleep(delay)

leg_control(28, 60, -28, 51.9, -28, 51.9, 28, 60)

time.sleep(delay)

leg_control(27, 60, -27, 51.4, -27, 51.4, 27, 60)

time.sleep(delay)

leg_control(26, 60, -26, 50.9, -26, 50.9, 26, 60)

time.sleep(delay)

leg_control(25, 60, -25, 50.5, -25, 50.5, 25, 60)

time.sleep(delay)

leg_control(24, 60, -24, 50, -24, 50, 24, 60)

time.sleep(delay)

leg_control(23, 60, -23, 49.6, -23, 49.6, 23, 60)

time.sleep(delay)

leg_control(22, 60, -22, 49.2, -22, 49.2, 22, 60)

time.sleep(delay)

leg_control(21, 60, -21, 48.8, -21, 48.8, 21, 60)

time.sleep(delay)

leg_control(20, 60, -20, 48.4, -20, 48.4, 20, 60)

time.sleep(delay)

leg_control(19, 60, -19, 48.1, -19, 48.1, 19, 60)

time.sleep(delay)

leg_control(18, 60, -18, 47.8, -18, 47.8, 18, 60)

time.sleep(delay)

leg_control(17, 60, -17, 47.5, -17, 47.5, 17, 60)

time.sleep(delay)

leg_control(16, 60, -16, 47.2, -16, 47.2, 16, 60)

time.sleep(delay)

leg_control(15, 60, -15, 46.9, -15, 46.9, 15, 60)

time.sleep(delay)

leg_control(14, 60, -14, 46.7, -14, 46.7, 14, 60)

time.sleep(delay)

leg_control(13, 60, -13, 46.4, -13, 46.4, 13, 60)

time.sleep(delay)

leg_control(12, 60, -12, 46.2, -12, 46.2, 12, 60)

time.sleep(delay)

leg_control(11, 60, -11, 46, -11, 46, 11, 60)

time.sleep(delay)

leg_control(10, 60, -10, 45.8, -10, 45.8, 10, 60)

time.sleep(delay)

leg_control(9, 60, -9, 45.7, -9, 45.7, 9, 60)

time.sleep(delay)

leg_control(8, 60, -8, 45.5, -8, 45.5, 8, 60)

time.sleep(delay)

leg_control(7, 60, -7, 45.4, -7, 45.4, 7, 60)

time.sleep(delay)

leg_control(6, 60, -6, 45.3, -6, 45.3, 6, 60)

time.sleep(delay)

leg_control(5, 60, -5, 45.2, -5, 45.2, 5, 60)

time.sleep(delay)

leg_control(4, 60, -4, 45.1, -4, 45.1, 4, 60)

time.sleep(delay)

leg_control(3, 60, -3, 45.1, -3, 45.1, 3, 60)

time.sleep(delay)

leg_control(2, 60, -2, 45, -2, 45, 2, 60)

time.sleep(delay)

leg_control(1, 60, -1, 45, -1, 45, 1, 60)

time.sleep(delay)

leg_control(0, 60, 0, 45, 0, 45, 0, 60)

time.sleep(delay)

leg_control(-1, 60, 1, 45, 1, 45, -1, 60)

time.sleep(delay)

leg_control(-2, 60, 2, 45, 2, 45, -2, 60)

time.sleep(delay)

leg_control(-3, 60, 3, 45.1, 3, 45.1, -3, 60)

time.sleep(delay)

leg_control(-4, 60, 4, 45.1, 4, 45.1, -4, 60)

time.sleep(delay)

leg_control(-5, 60, 5, 45.2, 5, 45.2, -5, 60)

time.sleep(delay)

leg_control(-6, 60, 6, 45.3, 6, 45.3, -6, 60)

time.sleep(delay)

leg_control(-7, 60, 7, 45.4, 7, 45.4, -7, 60)

time.sleep(delay)

leg_control(-8, 60, 8, 45.5, 8, 45.5, -8, 60)

time.sleep(delay)

leg_control(-9, 60, 9, 45.7, 9, 45.7, -9, 60)

time.sleep(delay)

leg_control(-10, 60, 10, 45.8, 10, 45.8, -10, 60)

time.sleep(delay)

leg_control(-11, 60, 11, 46, 11, 46, -11, 60)

time.sleep(delay)

leg_control(-12, 60, 12, 46.2, 12, 46.2, -12, 60)

time.sleep(delay)

leg_control(-13, 60, 13, 46.4, 13, 46.4, -13, 60)

time.sleep(delay)

leg_control(-14, 60, 14, 46.7, 14, 46.7, -14, 60)

time.sleep(delay)

leg_control(-15, 60, 15, 46.9, 15, 46.9, -15, 60)

time.sleep(delay)

leg_control(-16, 60, 16, 47.2, 16, 47.2, -16, 60)

time.sleep(delay)

leg_control(-17, 60, 17, 47.5, 17, 47.5, -17, 60)

time.sleep(delay)

leg_control(-18, 60, 18, 47.8, 18, 47.8, -18, 60)

time.sleep(delay)

leg_control(-19, 60, 19, 48.1, 19, 48.1, -19, 60)

time.sleep(delay)

leg_control(-20, 60, 20, 48.4, 20, 48.4, -20, 60)

time.sleep(delay)

leg_control(-21, 60, 21, 48.8, 21, 48.8, -21, 60)

time.sleep(delay)

leg_control(-22, 60, 22, 49.2, 22, 49.2, -22, 60)

time.sleep(delay)

leg_control(-23, 60, 23, 49.6, 23, 49.6, -23, 60)

time.sleep(delay)

leg_control(-24, 60, 24, 50, 24, 50, -24, 60)

time.sleep(delay)

leg_control(-25, 60, 25, 50.5, 25, 50.5, -25, 60)

time.sleep(delay)

leg_control(-26, 60, 26, 50.9, 26, 50.9, -26, 60)

time.sleep(delay)

leg_control(-27, 60, 27, 51.4, 27, 51.4, -27, 60)

time.sleep(delay)

leg_control(-28, 60, 28, 51.9, 28, 51.9, -28, 60)

time.sleep(delay)

leg_control(-29, 60, 29, 52.5, 29, 52.5, -29, 60)

time.sleep(delay)

leg_control(-30, 60, 30, 53, 30, 53, -30, 60)

time.sleep(delay)

leg_control(-31, 60, 31, 53.6, 31, 53.6, -31, 60)

time.sleep(delay)

leg_control(-32, 60, 32, 54.2, 32, 54.2, -32, 60)

time.sleep(delay)

leg_control(-33, 60, 33, 54.9, 33, 54.9, -33, 60)

time.sleep(delay)

leg_control(-34, 60, 34, 55.6, 34, 55.6, -34, 60)

time.sleep(delay)

leg_control(-35, 60, 35, 56.3, 35, 56.3, -35, 60)

time.sleep(delay)

leg_control(-36, 60, 36, 57, 36, 57, -36, 60)

time.sleep(delay)

leg_control(-37, 60, 37, 57.8, 37, 57.8, -37, 60)

time.sleep(delay)

leg_control(-38, 60, 38, 58.6, 38, 58.6, -38, 60)

time.sleep(delay)

leg_control(-39, 60, 39, 59.4, 39, 59.4, -39, 60)

time.sleep(delay)

leg_control(-40, 60, 40, 60, 40, 60, -40, 60)

time.sleep(delay)